#include "bsp_queue.h"

uint8_t InitQueue(linkQueue_t *p_lq)
{
    p_lq->cur_pos = p_lq->pos = 0;
    for(int i= 0;i<MAXNODE-1;i++)
    {
        p_lq->spinode[i].next = &(p_lq->spinode[i+1]);
        p_lq->spinode[i].spidata.flag = Q_Used;
    }
    p_lq->spinode[MAXNODE-1].next = &(p_lq->spinode[0]);
    p_lq->spinode[MAXNODE-1].spidata.flag = Q_Used;
    return Ok;
}
uint8_t InQueue(linkQueue_t *p_lq,uint8_t *data,uint16_t len, uint8_t *ID)
{
    if((len>QMAXLEN)||(len==0))
    {
        return BADPARA;
    }
    p_lq->pos++;
    if(p_lq->pos>=MAXNODE)
    {
        p_lq->pos = 0;
    }
    if(p_lq->spinode[p_lq->pos].spidata.flag == Q_Used)
    {
        *ID = p_lq->pos;
        for(int i= 0;i<len;i++)
        {
            p_lq->spinode[p_lq->pos].spidata.data[i] = data[i];
        }
        p_lq->spinode[p_lq->pos].spidata.len = len;
        p_lq->spinode[p_lq->pos].spidata.flag = Q_Pending;
    }
    else
    {
        if(p_lq->pos == 0)
        {
            p_lq->pos = 9;
        }
        else
        {
            p_lq->pos--;
        }       
        return QOVERFLOW;
    }
    return Ok;
}
uint8_t OutQueue(linkQueue_t *p_lq, uint8_t *data,uint16_t *p_len, uint8_t *ID)
{
    if(p_lq->cur_pos!= p_lq->pos)
    {
        p_lq->cur_pos++;
        if(p_lq->cur_pos>=MAXNODE)
        {
            p_lq->cur_pos = 0;
        }
        *p_len = p_lq->spinode[p_lq->cur_pos].spidata.len;
        *ID = p_lq->cur_pos;
        for(int i= 0;i<*p_len ;i++)
        {
            data[i] = p_lq->spinode[p_lq->cur_pos].spidata.data[i];
        }
        p_lq->spinode[p_lq->cur_pos].spidata.flag = Q_Used;
    }
    else
    {
        return QEmpty;
    }
    return Ok;
}
uint8_t checkBuffer(linkQueue_t *p_lq, uint8_t ID)
{
    return p_lq->spinode[ID].spidata.flag;
}
uint8_t checkQueueEmpty(linkQueue_t *p_lq)
{
    if(p_lq->cur_pos == p_lq->pos)
    {
        return QEmpty;
    }
    return Ok;
}